/** @file m_grahamscan.cpp
	@brief Graham Scan on 2D and 3D points */

#include "math/m_grahamscan.h"

using namespace G3;

//==============================================================================
// Constructors and Destructors
//==============================================================================
G3::GRAHAM_SCAN2F::GRAHAM_SCAN2F () {
    VertexList = NULL;
    NumVertices = 0;
}

G3::GRAHAM_SCAN2F::~GRAHAM_SCAN2F () {
    if (!Indices.empty ())
        Indices.clear ();
}

G3::GRAHAM_ANGLE2F::GRAHAM_ANGLE2F (ushort VertexID, float Angle, float Distance) {
	this->VertexID = VertexID;
	this->Angle = Angle;
	this->Distance = Distance;
}

//==============================================================================
// Operators
//==============================================================================
bool G3::GRAHAM_ANGLE2F::operator< (const GRAHAM_ANGLE2F &GrahamAngle) const {
	if (Angle == GrahamAngle.Angle)
		return (Distance < GrahamAngle.Distance);
	return (Angle < GrahamAngle.Angle);
}

//==============================================================================
// Performs a Graham Scan on the list of vertices
//==============================================================================
bool G3::GRAHAM_SCAN2F::Scan (G3::VEC2F *Vertices, int NumVerts) {
	multiset<GRAHAM_ANGLE2F> AngleList;
	multiset<GRAHAM_ANGLE2F>::iterator ali;
	ushort P = 0;
	G3::VEC2F Min, vp;
	int i = 0;

	if (!Indices.empty ()) {
		Indices.clear ();
		NumVertices = 0;
		VertexList = NULL;
	}

	if (Vertices != NULL && NumVerts > 2) {
		VertexList = Vertices;
		NumVertices = NumVerts;

		if (NumVerts > 0) {
			Min = Vertices [0];

			// Find the pivot point (The leftmost and bottommost point)
			for (i=0; i<NumVerts; i++) {
				if (Vertices [i].x < Min.x ||
					(Vertices [i].x == Min.x &&
					Vertices [i].y < Min.y)) {

					Min.x = Vertices [i].x;
					Min.y = Vertices [i].y;
					P = i;
				}
			}

			// Sort vertices by their angles
			for (i=0; i<NumVerts; i++) {
				if (i == P)
					continue;

				vp = Vertices [i] - Vertices [P];

				// NOTE:: We only need to know the slope (tangent of the angle) for sorting
				//		  That is because the pivot is the leftmost point
				AngleList.insert (GRAHAM_ANGLE2F (i, vp.y / vp.x, vp.GetLength ()));
			}

			// Walk through the points
			Indices.push_back (P);
			Indices.push_back (AngleList.begin ()->VertexID);
			for (ali=AngleList.begin (), ali++; ali != AngleList.end (); ali++) {
				// Is it a right turn?
				while (Indices.size () > 1 &&
					   CrossProduct (Vertices [Indices [Indices.size () - 2]], Vertices [Indices [Indices.size () - 1]], Vertices [ali->VertexID]) <= 0) {
					// If so, then remove the last vertex from the vector
					// - this is not a part of the Graham hull
					Indices.erase (Indices.begin () + Indices.size () - 1);
				}
				// Add another vertex
				Indices.push_back (ali->VertexID);
			}

			return true;
		}
	}
	return false;
}
